/*
    Nios-sim - one simple NIOSII simulator only for personal interest and fun.
    Copyright (C) 2010  chysun2000@gmail.com

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "public.h"
#include "nor_flash.h"

typedef enum {
	S_INVALID = 0x00,
	S0 = 0x01,
	S1 = 0x02,
	S2 = 0x03,
	S3 = 0x04,
	S4 = 0x05,
	S5 = 0x06,
	S6 = 0x07,
	S7 = 0x08,
	S8 = 0x09,
	S9 = 0x0A
} SREC_TYPE;

static struct image_info image_info;

static int32_t is_line_end(char data)
{
	if (data == '\n'){
		return 0;
	}
	else{
		return -1;
	}
}

static void get_line(FILE * fp, char * buf, int32_t buf_len, int32_t * line_len)
{
	char data = '\0';
	int32_t index = 0;
	
	memset(buf, 0x00, buf_len);
	while(1){
		if(feof(fp) != 0 || ferror(fp) != 0){
			return ;
		}
		
		data = '\0';
		if (fread(&data, 1, 1, fp) == 1){
			if (is_line_end(data) != 0){
				buf[index++] = data;
				*line_len = index;
			}
			else {
				return ;
			}
		}
		else {
			return ;
		}
	}
}

static SREC_TYPE get_srec_type(char * buf)
{
	if (buf[1] >= 0x30 && buf[1] <= 0x39){
		return S0 + buf[1] - 0x30;
	}
	else{
		return S_INVALID;
	}
}

static uint32_t __factors [] = {
	1, 
	16, 
	256,
	16 * 16 * 16, 
	16 * 16 * 16 * 16,
	16 * 16 * 16 * 16 * 16,
	16 * 16 * 16 * 16 * 16 * 16,
	16 * 16 * 16 * 16 * 16 * 16 * 16
};

static uint32_t single_ascii_to_hex(char data)
{
	if (data >= 0x30 && data <= 0x39)
		return data - 0x30;
	if (data >= 0x41 && data <= 0x46)
		return data - 0x41 + 10;
	if (data >= 0x61 && data <= 0x66)
		return data - 0x61 + 10;
	return 0;
}

uint32_t ascii_to_hex(char * buf, uint32_t buf_len)
{
	uint32_t ret = 0x0000;
	int32_t i = 0;
	for (i=0;i<buf_len;i++){
		ret = ret + (single_ascii_to_hex(buf[i]) * __factors[buf_len - i - 1]);
	}
	return ret;
}

static uint32_t get_srec_data_count(char * buf)
{
	uint32_t count = 0;
	count = ascii_to_hex(buf + 2, 2);
	return count;
}

static uint32_t to_le(uint32_t data)
{
	uint32_t ret = 0;
	ret = ret | ((data & 0xFF000000) >> 24);
	ret = ret | ((data & 0x00FF0000) >> 8);
	ret = ret | ((data & 0x0000FF00) << 8);
	ret = ret | ((data & 0x000000FF) << 24);
	return ret;
}

#define S0_DATA_OFFSET (8)
#define S0_ADDR_CRC_CNT (3)
static uint32_t handle_S0(char * buf, uint32_t data_count, uint32_t * mem_base)
{
	int32_t i = 0;
	char * data_buf = buf;
	
	data_buf = data_buf + S0_DATA_OFFSET;
	for (i=0;i<data_count - S0_ADDR_CRC_CNT; i++){
		data_buf += 2;
	}
	return 0;
}

#define S3_ADDR_OFFSET (4)
#define S3_DATA_OFFSET (S3_ADDR_OFFSET + 8)
#define S3_ADDR_CRC_CNT (5)

static uint32_t get_S3_address(char * buf)
{
	uint32_t ret = 0;
	buf = buf + S3_ADDR_OFFSET;
	ret = ascii_to_hex(buf, 8);
	return ret;
}

static uint32_t handle_S3(char * buf, uint32_t data_count, uint32_t * mem_base, 
							  uint32_t base_addr)
{
	uint32_t start_addr = 0;
	uint32_t offset = 0;
	uint32_t data = 0;
	char * data_buf = buf;
	int32_t i;
	
	start_addr = get_S3_address(buf);

	if (base_addr == 0){
		offset = 0;
	}
	else{
		offset = (start_addr - base_addr)/4;
	}
	data_buf = data_buf + S3_DATA_OFFSET;
	for (i=0;i<(data_count-S3_ADDR_CRC_CNT)/4;i++){
		data = to_le(ascii_to_hex(data_buf, 8));
		data_buf += 8;
		mem_base[offset + i] = data;
	}

	return start_addr;
}

#define S2_ADDR_OFFSET (4)
#define S2_DATA_OFFSET (S2_ADDR_OFFSET + 6)
#define S2_ADDR_CRC_CNT (4)

static uint32_t get_S2_address(char * buf)
{
	uint32_t ret = 0;
	buf = buf + S2_ADDR_OFFSET;
	ret = ascii_to_hex(buf, 6);
	return ret;
}

static uint32_t handle_S2(char * buf, uint32_t data_count, uint32_t * mem_base, 
							  uint32_t base_addr)
{
	uint32_t start_addr = 0;
	uint32_t offset = 0;
	uint32_t data = 0;
	char * data_buf = buf;
	int32_t i;
	
	start_addr = get_S2_address(buf);

	if (base_addr == 0){
		offset = 0;
	}
	else{
		offset = (start_addr - base_addr)/4;
	}
	
	data_buf = data_buf + S2_DATA_OFFSET;
	for (i=0;i<(data_count-S2_ADDR_CRC_CNT)/4;i++){
		data = to_le(ascii_to_hex(data_buf, 8));
		data_buf += 8;
		mem_base[offset + i] = data;
	}

	return start_addr;
}


#define S7_DATA_OFFSET (4)
#define S7_CRC_CNT (2)
static uint32_t handle_S7(char * buf, uint32_t data_count, uint32_t * mem_base)
{
	char * data_buf = buf;

	data_buf = data_buf + S7_DATA_OFFSET;
	return ascii_to_hex(data_buf, 8);
}

#define S8_DATA_OFFSET (4)
#define S8_CRC_CNT (2)
static uint32_t handle_S8(char * buf, uint32_t data_count, uint32_t * mem_base)
{
	char * data_buf = buf;

	data_buf = data_buf + S8_DATA_OFFSET;
	return ascii_to_hex(data_buf, 6);
}

static void dummy_srec_handler(char * name)
{
	printf("%s: for %s\n",__func__, name);
}

static void handle_srec_line(char * buf, int line_len, uint32_t * mem_base)
{
	SREC_TYPE srec_type = S_INVALID;
	uint32_t data_count = 0;
	uint32_t start_addr = 0;

	srec_type = get_srec_type(buf);
	data_count =  get_srec_data_count(buf);

	switch(srec_type){
	case S0:
		handle_S0(buf, data_count, mem_base);
		break;
	case S1:
		dummy_srec_handler("S1");
		break;
	case S2:
		if (image_info.base_addr == 0){
			image_info.base_addr = handle_S2(buf, data_count, mem_base, image_info.base_addr);
		}
		else {
			handle_S2(buf, data_count, mem_base, image_info.base_addr);
		}
		break;
	case S3:
		if (image_info.base_addr == 0){
			image_info.base_addr = handle_S3(buf, data_count, mem_base, image_info.base_addr);
		}
		else {
			handle_S3(buf, data_count, mem_base, image_info.base_addr);
		}
		break;
	case S4:
		dummy_srec_handler("S4");
		break;
	case S5:
		dummy_srec_handler("S5");
		break;
	case S6:
		dummy_srec_handler("S6");
		break;
	case S7:
		start_addr = handle_S7(buf, data_count, mem_base);
		set_image_entry_addr(start_addr);
		break;
	case S8:
		start_addr = handle_S8(buf, data_count, mem_base);
		set_image_entry_addr(start_addr);
		break;
	case S9:
		dummy_srec_handler("S9");
		break;
	default:
		return;
	}
	return;
}

#define SREC_LINE_LENGTH (515)
static char data_buf[SREC_LINE_LENGTH] = {0};

static void load_srec(const char * image_path_name, uint32_t * image_addr)
{
	FILE * fp = NULL;
	int32_t line_len = 0;

	fp = fopen(image_path_name, "r");
	if (fp != NULL){
		while(1){
			memset(data_buf, 0x00, SREC_LINE_LENGTH);
			line_len = 0;
			get_line(fp, data_buf, SREC_LINE_LENGTH, &line_len);
			if (line_len == 0){
				break;
			}
			else
			{
				handle_srec_line(data_buf, line_len, image_addr);
			}
		}
		fclose(fp);
	}
	else{
		printf("ERROR--> can not open image at %s\n", image_path_name);
	}
}

void load_image(void)
{
	switch(image_info.image_format){
	case SREC_FMT:
		load_srec(image_info.path_name, image_info.mem_base);
		break;
	default:
		break;
	}
}

void alloc_image_mem(void)
{
	char * address = NULL;
	struct image_info * info = &image_info;
	uint32_t temp = 0;
	
	address = (char *)malloc(info->mem_size);
	temp =  info->mem_size % 4;
	if (temp != 0){
		address = address + temp;
	}
	info->mem_base = (uint32_t *)address;
}

void set_image_pathname(char * pathname)
{
	image_info.path_name = pathname;
}

void set_image_format(IMG_FORMAT format)
{
	image_info.image_format = format;
}

void set_image_memsize(char * size)
{
	int32_t str_len = strlen(size);
	char * temp = NULL;

	if (size[str_len - 1] == 'M' || size[str_len - 1] == 'm'){
		temp = malloc(str_len);
		memset(temp, 0x00, str_len);
		memcpy(temp, size, str_len);
		temp[str_len - 1] = '\0';
		image_info.mem_size = atoi(temp) * 1024 * 1024;
		free(temp);
	}
}

void set_image_entry_addr(uint32_t addr)
{
	image_info.entry_addr = addr;
}

void set_image_base_addr(char * addr)
{
	sscanf(addr, "0x%X\n", &image_info.base_addr);
}
static char cmd_line[256] = {0};
static uint32_t initrd_start = 0;
static uint32_t initrd_size = 0;
static char fs_image[256] = {0};

void print_image_info(void)
{
	printf("--------------------------------------------------\n");
	printf(" NIOS Simulator (Built at %s-%s)\n",__DATE__,__TIME__);
	printf("--------------------------------------------------\n");
	printf("Image File:%s\n",image_info.path_name);
	
	if (image_info.image_format == SREC_FMT){
		printf("Format:SREC\n");
	}

	printf("Mem size:0x%08X, loading at %p\n", image_info.mem_size, 
		   image_info.mem_base);
	printf("Base address:0x%08X, Entry address:0x%08X\n",image_info.base_addr,
		   image_info.entry_addr);
	printf("Set command line:%s\n", cmd_line);
	printf("Initrd: 0x%08x size:0x%08x\n",initrd_start, initrd_size);
	printf("rootfs: %s \n",fs_image);
	printf("--------------------------------------------------\n");
}

struct image_info * get_image_info(void)
{
	return &image_info;
}

static struct symbol_obj * sym_hash[256] = {NULL};

static struct symbol_obj * get_head_list(uint32_t addr)
{
	return sym_hash[addr % 256];
}

static void insert_symbol_obj(struct symbol_obj * list, struct symbol_obj * obj)
{
	if (list == NULL){
		sym_hash[obj->addr % 256] = obj;
	}
	else{
		while(list != NULL){
			if (list->next != NULL){
				list = list->next;
			}
			else{
				break;
			}
			
		}
		list->next = obj;
	}
}

static struct symbol_obj * alloc_symbol_obj(char * addr, char * name)
{
	struct symbol_obj * obj = NULL;
	uint32_t size = 0;
	
	obj = (struct symbol_obj *)malloc(sizeof(struct symbol_obj));
	memset(obj, 0x00, sizeof(struct symbol_obj));

	if (obj != NULL){
		obj->addr = ascii_to_hex(addr, 8);
		obj->next = NULL;
		size = strlen(name) + 1;
		obj->sym_name = (char *)malloc(size);
		if (obj->sym_name != NULL){
			memset(obj->sym_name, 0x00, size);
			strncpy(obj->sym_name, name, size - 1);
		}
		else{
			printf("[ERROR] %s --> can not allocate symname %s \n",__func__, name);
		}
	}

	return obj;
}

static void handle_symbol_line(char * line)
{
	char addr[10] = {0};
	char sym_name[64] = {0};
	char type[2] = {0};
	struct symbol_obj * obj = NULL;
	struct symbol_obj * list = NULL;

	memset(sym_name, 0x00, 64);
	sscanf(line, "%s %s %s\n", addr, type, sym_name);
	obj = alloc_symbol_obj(addr, sym_name);

	if (obj != NULL){
		list = get_head_list(obj->addr);
		insert_symbol_obj(list, obj);
	}
}

void load_symbol_file(char * symbol_file)
{
	char line[256] = {0};
	int32_t line_len = 0;
	FILE * fp = NULL;
	
	memset(sym_hash, 0x00, sizeof(sym_hash));
	fp = fopen(symbol_file, "r");
	if (fp != NULL){
		while(1){
			memset(line, 0x00, 256);
			line_len = 0;
			get_line(fp, line, 256, &line_len);
			if (line_len == 0){
				break;
			}
			else{
				line[line_len] = '\0';
				handle_symbol_line(line);
			}
		}
		fclose(fp);
	}
}

static struct symbol_obj * find_symbol_obj(struct symbol_obj * list, uint32_t addr)
{
	struct symbol_obj * ret = NULL;
	
	ret = list;
	while(ret != NULL){
		if (ret->addr == addr){
			break;
		}
		else{
			ret = ret->next;
		}
	}
	return ret;
}

struct symbol_obj * get_symbol(uint32_t addr)
{
	struct symbol_obj * ret = NULL;
	struct symbol_obj * lst = NULL;
	
	lst = get_head_list(addr);
	if (lst != NULL){
		ret = find_symbol_obj(lst, addr);
	}

	return ret;
}

void set_cmdline(char * optarg)
{
	strcpy(cmd_line,optarg);
}

void init_cmdline(struct NIOS_CPU * cpu)
{

	char * addr = (char *)image_info.mem_base;
	
	/* R7 is stored the start address of command line */
	memcpy((char *)(addr + 0x700000), cmd_line, 255);
	/* Set magic number.
	 * this is copied from arch/nios2/kernel/setup.c <setup_arch>
	 */
	if (cmd_line[0] != '\0'){
		cpu->gp_regs[4] = 0x534f494e;
		cpu->gp_regs[7] = 0xF00000;
	}
	
}

void set_initrd(char * optarg)
{
	sscanf(optarg,"0x%08x,0x%08x",&initrd_start,&initrd_size);
}

void init_initrd(struct NIOS_CPU * cpu)
{
	cpu->gp_regs[5] = initrd_start & 0xFFFFFFFC;
	cpu->gp_regs[6] = (initrd_start + initrd_size) & 0xFFFFFFFC;
}

void set_fs_image(char * optarg)
{
	sscanf(optarg,"%s",fs_image);
}

void init_fs_image(struct NIOS_CPU * cpu)
{
	int32_t fd = open(fs_image,O_RDWR);
	int32_t size = 0;
	uint8_t buf [256] = {0};
	uint8_t * mem_load_addr = NULL;

	/* set the load memory address */
	mem_load_addr = nor_flash_mem_addr();

	if (fd){
		printf("loading rootfs at %p\n", mem_load_addr);
		while((size = read(fd,buf, 256)) > 0){
			memcpy(mem_load_addr, buf, size);
			mem_load_addr += size;
		}

		close(fd);
	}
	else {
		printf("Can not load rootfs image at %s\n",fs_image);
	}
}

/*----------------------------------------------------------------------------*/

